import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import Voronoi, voronoi_plot_2d
from sklearn.neighbors import KDTree
from shapely.geometry import Polygon, Point
import networkx as nx
from heapq import heappop, heappush
import cv2
import numpy as np
import matplotlib.pyplot as plt
import networkx as nx
plt.rcParams['font.sans-serif'] = ['WenQuanYi Micro Hei', 'SimHei']  # 选择适用的字体
plt.rcParams['axes.unicode_minus'] = False  # 解决坐标轴负号显示问题

node_type_dic = {0:'交叉口',1:'设备',2:'巡检点',3:'障碍物',4:'起始点',5:'终止点',6:'无效点'}
edge_type_dic = {0:'路线',1:'人为加线',2:'无效路线',3:'巡检路线'}

node_type_color = {0:'black',1:'green',2:'yellow',3:'red',4:'orange',5:'orange',6:'gray'}
edge_type_color = {0:'black',1:'black',2:'gray',3:'blue'}

MAX_WEIGHT = -1
retained = [1,2,3,4]
pri_path = [1,2,1,3]
#这个字典已经经过了图处理和计算
dic = {'node':{1:{'pos':(10,90),'type':0,'weight':10},
               2:{'pos':(30,90),'type':0,'weight':10},
               3:{'pos':(10,60),'type':0,'weight':10},
               4:{'pos':(30,60),'type':0,'weight':10},
               10:{'pos':(60,30),'type':0,'weight':10},
               8:{'pos':(90,30),'type':0,'weight':10},
               13:{'pos':(60,10),'type':0,'weight':10},
               15:{'pos':(90,10),'type':0,'weight':10}},
       'edge': {1:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[1,2],'pos':[(10,20),(50,50)]},
                2:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[1,3],'pos':[(10,20),(50,50)]},
                3:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[2,4],'pos':[(10,20),(50,50)]},
                4:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[3,4],'pos':[(10,20),(50,50)]},
                5:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[4,10],'pos':[(10,20),(50,50)]},
                6:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[10,8],'pos':[(10,20),(50,50)]},
                7:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[10,13],'pos':[(10,20),(50,50)]},
                8:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[13,15],'pos':[(10,20),(50,50)]},
                9:{'type':0,'cost':{"tol":100,'len':50,'cur':50},'endpoint':[8,15],'pos':[(10,20),(50,50)]}},
       'points':[1,2,3,4,10,8,13],
       'key_points':[1,3,2],
       'obstacle_points':[],
       'start_point':1,
       'goal_point':3}

data = {
    "nodes": [
  {
      "id": "a9e726a1-e8c2-4720-a8fe-3b82dc27c56a",
      "customID": 0,
      "type": "cross",
      "position": {
          "x": 340,
          "y": 420
      },
      "isKey": "false"
  },
  {
      "id": "82df58d2-300d-4802-9233-1797aafe8b79",
      "customID": 1,
      "type": "cross",
      "position": {
          "x": 540,
          "y": 230
      },
      "isKey": "false"
  },
  {
      "id": "cc371c7a-3f76-43be-8511-f97b623288d0",
      "customID": 2,
      "type": "cross",
      "position": {
          "x": 770,
          "y": 220
      },
      "isKey": "false"
  },
  {
      "id": "1720e17b-50f2-4ee1-a215-e9842eb7dbfb",
      "customID": 3,
      "type": "start",
      "position": {
          "x": 210,
          "y": 600
      },
      "isKey": "false"
  },
  {
      "id": "40d5679e-d312-4aa9-8611-85ac492c916e",
      "customID": 4,
      "type": "cross",
      "position": {
          "x": 540,
          "y": 420
      },
      "isKey": "false"
  },
  {
      "id": "d293939a-b18d-4ea1-b422-f05b73ed85fe",
      "customID": 5,
      "type": "cross",
      "position": {
          "x": 770,
          "y": 420
      },
      "isKey": "false"
  },
  {
      "id": "6d67d2e8-4775-4b77-bc06-6ab4db9ca29b",
      "customID": 6,
      "type": "cross",
      "position": {
          "x": 540,
          "y": 640
      },
      "isKey": "false"
  },
  {
      "id": "62e2ab0c-2d03-4e98-a618-7c33939c61c7",
      "customID": 7,
      "type": "cross",
      "position": {
          "x": 1030,
          "y": 470
      },
      "isKey": "false"
  },
  {
      "id": "d4618d40-e723-4ce0-b225-1c7030983eda",
      "customID": 8,
      "type": "end",
      "position": {
          "x": 1030,
          "y": 210
      },
      "isKey": "false"
  },
  {
      "id": "d295c2a3-25ed-4e24-98cb-dddacd6024df",
      "customID": 9,
      "type": "device",
      "position": {
          "x": 223,
          "y": 368
      },
      "isKey": "true"
  },
  {
      "id": "95ee352c-5720-4db4-9711-f611e6fbd18a",
      "customID": 10,
      "type": "device",
      "position": {
          "x": 379,
          "y": 233
      },
      "isKey": "true"
  },
  {
      "id": "6c7484b2-a169-4ade-a146-2f4506c80965",
      "customID": 11,
      "type": "device",
      "position": {
          "x": 430,
          "y": 430
      },
      "isKey": "true"
  },
  {
      "id": "e5c4b741-cb3c-49fe-9541-4e171449afd3",
      "customID": 12,
      "type": "device",
      "position": {
          "x": 780,
          "y": 330
      },
      "isKey": "true"
  },
  {
      "id": "5cb2e33a-7994-40c6-a3af-8cb4812fb447",
      "customID": 13,
      "type": "device",
      "position": {
          "x": 768,
          "y": 615
      },
      "isKey": "true"
  },
  {
      "id": "332b7eb9-ef9a-4db5-807e-eb8472da6ce2",
      "customID": 14,
      "type": "device",
      "position": {
          "x": 930,
          "y": 230
      },
      "isKey": "true"
  }
],
    "edges":[
  {
      "id": "3aa28c0d-fd68-4458-8df0-1eb72d2fb37d",
      "connector": {
          "name": "smooth"
      },
      "vertices": [
          {
              "x": 300,
              "y": 510
          },
          {
              "x": 250,
              "y": 530
          }
      ],
      "target": "1720e17b-50f2-4ee1-a215-e9842eb7dbfb",
      "source": "a9e726a1-e8c2-4720-a8fe-3b82dc27c56a",
      "lenCost": 243.04428100585938,
      "curCost": 0
  },
  {
      "id": "964b25d3-57bd-4609-a406-d7b80ae3fe19",
      "connector": "normal",
      "vertices": [],
      "target": "1720e17b-50f2-4ee1-a215-e9842eb7dbfb",
      "source": "6d67d2e8-4775-4b77-bc06-6ab4db9ca29b",
      "lenCost": 321.4031677246094,
      "curCost": 0
  },
  {
      "id": "3c2eedd2-07b2-41e8-b12e-c6451b2b8b85",
      "connector": "normal",
      "vertices": [],
      "target": "40d5679e-d312-4aa9-8611-85ac492c916e",
      "source": "82df58d2-300d-4802-9233-1797aafe8b79",
      "lenCost": 190,
      "curCost": 0
  },
  {
      "id": "6adab7b8-2606-40bf-8dd6-f72122e8c985",
      "connector": "normal",
      "vertices": [],
      "target": "cc371c7a-3f76-43be-8511-f97b623288d0",
      "source": "82df58d2-300d-4802-9233-1797aafe8b79",
      "lenCost": 230.21728515625,
      "curCost": 0
  },
  {
      "id": "df0e7c0f-cbcc-40b6-8a19-1a69c033891a",
      "connector": "normal",
      "vertices": [],
      "target": "40d5679e-d312-4aa9-8611-85ac492c916e",
      "source": "d293939a-b18d-4ea1-b422-f05b73ed85fe",
      "lenCost": 230,
      "curCost": 0
  },
  {
      "id": "ee429e24-812e-4611-be08-3c9d9a0aead5",
      "connector": {
          "name": "smooth"
      },
      "vertices": [
          {
              "x": 950,
              "y": 480
          },
          {
              "x": 890,
              "y": 430
          }
      ],
      "target": "d293939a-b18d-4ea1-b422-f05b73ed85fe",
      "source": "62e2ab0c-2d03-4e98-a618-7c33939c61c7",
      "lenCost": 281.15289306640625,
      "curCost": 0
  },
  {
      "id": "c1cb9ab0-f33c-4043-bcd3-214d3bad4641",
      "connector": "normal",
      "vertices": [],
      "target": "62e2ab0c-2d03-4e98-a618-7c33939c61c7",
      "source": "d4618d40-e723-4ce0-b225-1c7030983eda",
      "lenCost": 250.19992065429688,
      "curCost": 0
  },
  {
      "id": "d3b7f339-75c7-4482-ad01-1fe4ecbde57b",
      "connector": "normal",
      "vertices": [],
      "target": "40d5679e-d312-4aa9-8611-85ac492c916e",
      "source": "6d67d2e8-4775-4b77-bc06-6ab4db9ca29b",
      "lenCost": 220,
      "curCost": 0
  },
  {
      "id": "8bfe480f-717d-43e8-b845-15de697710bf",
      "connector": {
          "name": "smooth"
      },
      "vertices": [
          {
              "x": 250,
              "y": 390
          }
      ],
      "target": "a9e726a1-e8c2-4720-a8fe-3b82dc27c56a",
      "source": "d295c2a3-25ed-4e24-98cb-dddacd6024df",
      "lenCost": 136.52146911621094,
      "curCost": 0
  },
  {
      "id": "fd728a54-5ac6-4e51-8dae-9dec452cdb7f",
      "connector": {
          "name": "smooth"
      },
      "vertices": [],
      "target": "95ee352c-5720-4db4-9711-f611e6fbd18a",
      "source": "82df58d2-300d-4802-9233-1797aafe8b79",
      "lenCost": 164.67015075683594,
      "curCost": 0
  },
  {
      "id": "bb4a77ce-e26c-434a-a2f0-ae9075354514",
      "connector": {
          "name": "smooth"
      },
      "vertices": [
          {
              "x": 310,
              "y": 240
          },
          {
              "x": 200,
              "y": 310
          }
      ],
      "target": "d295c2a3-25ed-4e24-98cb-dddacd6024df",
      "source": "95ee352c-5720-4db4-9711-f611e6fbd18a",
      "lenCost": 268.9122619628906,
      "curCost": 0
  },
  {
      "id": "027aa33d-d161-4887-9c71-1c13da719ee5",
      "connector": {
          "name": "normal"
      },
      "vertices": [],
      "target": "6c7484b2-a169-4ade-a146-2f4506c80965",
      "source": "a9e726a1-e8c2-4720-a8fe-3b82dc27c56a",
      "lenCost": 74.50141906738281,
      "curCost": 0
  },
  {
      "id": "e07b4868-90d3-4d50-a004-2f928f46ee73",
      "connector": {
          "name": "normal"
      },
      "vertices": [],
      "target": "40d5679e-d312-4aa9-8611-85ac492c916e",
      "source": "6c7484b2-a169-4ade-a146-2f4506c80965",
      "lenCost": 113.5009765625,
      "curCost": 0
  },
  {
      "id": "62a60f82-e56e-4de2-96b1-54f3a67ec890",
      "connector": {
          "name": "normal"
      },
      "vertices": [],
      "target": "e5c4b741-cb3c-49fe-9541-4e171449afd3",
      "source": "cc371c7a-3f76-43be-8511-f97b623288d0",
      "lenCost": 94.50116729736328,
      "curCost": 0
  },
  {
      "id": "f0cd4b51-1ee2-40c8-99c7-2ab0c3c1fc22",
      "connector": {
          "name": "normal"
      },
      "vertices": [],
      "target": "d293939a-b18d-4ea1-b422-f05b73ed85fe",
      "source": "e5c4b741-cb3c-49fe-9541-4e171449afd3",
      "lenCost": 93.50118255615234,
      "curCost": 0
  },
  {
      "id": "c332821f-6085-4d64-96b0-665a39e98758",
      "connector": {
          "name": "smooth"
      },
      "vertices": [
          {
              "x": 740,
              "y": 610
          }
      ],
      "target": "5cb2e33a-7994-40c6-a3af-8cb4812fb447",
      "source": "6d67d2e8-4775-4b77-bc06-6ab4db9ca29b",
      "lenCost": 230.1471405029297,
      "curCost": 0
  },
  {
      "id": "d96a1dd7-e2f3-46e1-9483-aa35c30d6e89",
      "connector": {
          "name": "smooth"
      },
      "vertices": [
          {
              "x": 890,
              "y": 630
          }
      ],
      "target": "62e2ab0c-2d03-4e98-a618-7c33939c61c7",
      "source": "5cb2e33a-7994-40c6-a3af-8cb4812fb447",
      "lenCost": 334.8843994140625,
      "curCost": 0
  },
  {
      "id": "e683fcb1-d36c-4580-9681-ed634e3fce14",
      "connector": {
          "name": "normal"
      },
      "vertices": [],
      "target": "332b7eb9-ef9a-4db5-807e-eb8472da6ce2",
      "source": "cc371c7a-3f76-43be-8511-f97b623288d0",
      "lenCost": 144.50079345703125,
      "curCost": 0
  },
  {
      "id": "f45e5845-452d-41c5-83c1-1f5ce41b07e1",
      "connector": {
          "name": "normal"
      },
      "vertices": [],
      "target": "d4618d40-e723-4ce0-b225-1c7030983eda",
      "source": "332b7eb9-ef9a-4db5-807e-eb8472da6ce2",
      "lenCost": 113.5009765625,
      "curCost": 0
  }
]
}


class DrawGraph():
    def __init__(self):
        self.file = './resource/img_1.png'
        self.fromWeb = True
        self.node_id_list = dic['node'].keys()
        self.edge_id_list = dic['edge']
        self.M = self.jsonToMatr()
        self.node_pos = {i:dic['node'][i]['pos'] for i in self.node_id_list}
        self.node_inf = {i:{'pos':dic['node'][i]['pos'],'type':dic['node'][i]['type']} for i in self.node_id_list}
        self.edge_inf = {i:{'endpoint':dic['edge'][i]['endpoint'],'type':dic['edge'][i]['type']} for i in self.edge_id_list}


    #根据字典转化成邻接矩阵
    def jsonToMatr(self):
        #获取提取json部分信息
        node_id_list = self.node_id_list
        edge_id_list = self.edge_id_list

        #根据其他信息修改node的type

        #寻找字典中node最大的id
        size = max(node_id_list) + 1
        M = np.full((size,size), MAX_WEIGHT)

        #根据edge信息给M赋值
        for i in node_id_list:
            #i属于设备节点,后面应该type已经修改，无需在此处修改
            if i in dic['points']:
                dic['node'][i]['type'] = 1
            #i是巡检点
            if i in dic['key_points']:
                dic['node'][i]['type'] = 2

            # i是障碍物
            if i in dic['obstacle_points']:
                dic['node'][i]['type'] = 3

            # i是起始点
            if i == dic['start_point']:
                dic['node'][i]['type'] = 4

            # i是终止点
            if i == dic['goal_point']:
                dic['node'][i]['type'] = 5

            if i not in retained:
                dic['node'][i]['type'] = 6
            for j in node_id_list:
                for k in edge_id_list:
                    # 自身到自身的权值为节点权值
                    if i == j:
                        M[i][j] = dic['node'][i]['weight']
                        continue
                    # 若该边存在
                    elif (i in dic['edge'][k]['endpoint'] ) and (j in dic['edge'][k]['endpoint']):
                        M[i][j] = dic['edge'][k]['cost']['tol']
                        # 若i，j均不在retained中，则该边无效
                        if i not in retained or j not in retained:
                            dic['edge'][k]['type'] = 2
                            continue
                        # 若i，j均在pre_path中，则该边为巡检路线
                        if i in pri_path and j in pri_path:
                            dic['edge'][k]['type'] = 3
                            continue
        return M


    #根据给定两点id生成直线路径
    def getPath(self, node1, node2):
        path = []
        node1_pos = self.node_pos[node1]
        node2_pos = self.node_pos[node2]

        # 使用bresenham_line算法求路径
        x1 = node1_pos[0]
        y1 = node1_pos[1]
        x2 = node2_pos[0]
        y2 = node2_pos[1]

        dx = abs(x2 - x1)
        dy = abs(y2 - y1)

        sx = 1 if x1 < x2 else -1
        sy = 1 if y1 < y2 else -1

        err = dx - dy
        while True:
            path.append((x1, y1))
            if x1 == x2 and y1 == y2:
                break
            e2 = 2 * err
            if e2 > -dy:
                err -= dy
                x1 += sx
            if e2 < dx:
                err += dx
                y1 += sy
        type = 0
        for i in self.edge_id_list:
            if node1 in self.edge_inf[i]['endpoint'] and node2 in self.edge_inf[i]['endpoint']:
                return path,dic['edge'][i]['type']

    def createGridTest(self):

        # 获取所有节点和边id集合
        node_id_list = dic['node'].keys()
        edge_id_list = dic['edge']

        grid_size = 100
        grid_map = np.zeros((grid_size, grid_size))

        # 设置关键点
        node_pos = [dic['node'][i]['pos'] for i in node_id_list]

        # 标记关键点
        for point in node_pos:
            grid_map[point] = 1  # 1 表示关键点
        # 构建栅格图
        G = nx.grid_2d_graph(grid_size, grid_size)

        # 添加边的权重并生成路径
        path_list = []
        #连接两个节点，并添加边的权重
        for i in node_id_list:
            for j in node_id_list:
                if self.M[i][j] != MAX_WEIGHT and i != j:
                    path,type = self.getPath(i,j)
                    path_list.append([path,type,(i,j)])
                    edge_weight = self.M[i][j]
                    G.add_edge(dic['node'][i]['pos'], dic['node'][j]['pos'], weight=edge_weight)

        # 绘制栅格地图和路径
        plt.imshow(grid_map, cmap='gray_r', origin='lower')
        #绘制edge
        for path in path_list:
            color = edge_type_color[path[1]]
            path = path[0]
            for i in range(len(path)-1):
                plt.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]], color=color, linewidth=2)
        #绘制node
        for i in dic['node']:
            id = i

            node = dic['node'][i]
            point = node['pos']
            color = node_type_color[node['type']]

            note = ''
            if node['type'] == 4 or node['type'] == 5:
                note = '(' + node_type_dic[node['type']] + ')'
            plt.plot(point[0], point[1], marker = 'o', markersize = 10, color=color)
            plt.annotate(str(id) + note, (point[0], point[1]), textcoords="offset points", xytext=(5, 5), ha='center')

        plt.show()

    def run(self):
        self.createGridTest()
        print(self.M)